#include "Ultrasound_Handler.h"

extern union union32bits ultrasound_altitude;

void vfnUSound_SendBurst (void)
{
    uint8 u8us_counter;

    // Configure MTIM at a 80khz frequency to generate the u-sound burst
    vfnMTIM_PrescalerSelect (MTIM_PRESCALER_4);
    vfnMTIM_SetModulo (25);
    vfnMTIM_SetStatus (MTIM_ACTIVE);
    
    vfnTPM_SelectClock (TPM_CLK_BUS);
    for(u8us_counter = 0; u8us_counter < USOUND_BURST_SIZE; u8us_counter++)
    {
        // Toggle ultrasound tx pin
        USOUND_TX_PIN ^= 1;      
        
        // Restart MTIM and wait for overflow.
        vfnMTIM_Reset();
        while(!u8MTIM_CheckOvFlag());       
    }
    
    USOUND_TX_PIN = 0;
    vfnTPM_ChannelInterruptEnable (USOUND_TIMER_CHANNEL);
}

void vfnUSound_Init (void)
{
    USOUND_TX_PIN = 0;
    USOUND_TX_PIN_DD = GPIO_OUTPUT;
    
    vfnTPM_SetPrescalerDivider (TPM_PRESCALER_8);
    vfnTPM_ChannelInputCapture (USOUND_TIMER_CHANNEL, TPM_RISING_EDGE);
    vfnTPM_SelectClock (TPM_CLK_DISABLE);
    vfnTPM_SetModulo (0x0000); // Free running mode
      
}

interrupt 5 void TPM_Ch0_InputCapture (void)
{    
    vfnTPM_ClearChannelFlag (USOUND_TIMER_CHANNEL);
          
    ultrasound_altitude.word_access.u16access_low = u16TPM_CheckChannelValue (USOUND_TIMER_CHANNEL);
    
    /* Calculations assume a sound speed of 345 m/s */
    
    ultrasound_altitude.u32access  *= 69;
    
    ultrasound_altitude.u32access /= 125;
    
    ultrasound_altitude.word_access.u16access_low >>= 5;
      
    return;    
}


